package ai4games_tests.tests;

import game2dai.entities.MovingEntity;
import game2dai.entities.Vehicle;
import game2dai.maths.Vector2D;
import game2dai.steering.AutoPilot;

public class BitFlagTest {

	public static void main(String[] args) {

//		System.out.println();
//		
//		SB sb = new SB();
//		int w = 1;
//		for(int i = 0; i < 15; i++, w *= 2){
//			sb.setWeight(w, 123.45 + 10 * i);
//			sb.getWeight(w);
//		}
		
//		System.out.println();
//		
//		SB sb1 = new SB();
//	
//		sb1.enableBehaviours(SB.ARRIVE | SB.FLOCK | SB.PATH);
//		System.out.println("SB1 \n" + sb1);
//		
//		SB sb2 = (SB) sb1.clone();
//		sb2.setWeight(SB.ARRIVE, 121212);
//		sb2.disableBehaviours(SB.FLOCK);
//		System.out.println("SB2 \n" + sb2);
//		
//		System.out.println("SB1 \n" + sb1);


		Vehicle v0, v1;
		
		v0 = new Vehicle(new Vector2D(20,20), // position
				20, // collision radius
				new Vector2D(10,10), // velocity
				40, // maximum speed
				new Vector2D(1,-1), // heading
				1, // mass
				1,  // turning rate
				200 // max force
				);
		
		v1 = new Vehicle(new Vector2D(33, 45), // position
				20, // collision radius
				new Vector2D(10,10), // velocity
				40, // maximum speed
				new Vector2D(-1,0), // heading
				1, // mass
				1,  // turning rate
				200 // max force
				); 

		System.out.println(v0);
		System.out.println(v1);
		ensureZeroOverlap(v0,v1);
		System.out.println();
		System.out.println(v0);
		System.out.println(v1);
		
		
	}
	
	public static void ensureZeroOverlap(MovingEntity m0, MovingEntity m1){
		Vector2D toOther = Vector2D.sub(m0.pos(), m1.pos());
		double dist = toOther.length();
		double overlap = m0.colRadius() + m1.colRadius() - dist; 
		if(overlap >= 0){
			toOther.div(dist); // normalize
			System.out.println(m0.heading().dot(toOther) + "  " + m1.heading().dot(toOther) );
			System.out.println(m0.heading().dot(m1.heading()));
			if(m0.heading().dot(toOther) < 0 && m1.heading().dot(toOther) > 0){ // facing each other
				toOther.mult(overlap);
				m1.pos().add(-toOther.y, toOther.x); // move sideways
			}
			else {
				toOther.mult(overlap);
				m1.pos().add(-toOther.x, -toOther.y);
			}
		}
	}
	
	

}
